#ifndef _RUBBER_WHEEL_CAL_H
#define _RUBBER_WHEEL_CAL_H

/*
* 全向轮底盘状态
*    0     3
*	 1     2
* 
* self_pos
*         X
*         | 
*      z<-|
* Y<------ 
* 
* world_pos 
* Y   
* | 
* |<-z
* ------>X
* 
* 
*/

/**
 * @brief 位置信息
 * @param x x坐标,m
 * @param y y坐标,m
 * @param yaw 航向角,rad
 */
typedef struct
{
	float x;   // m
	float y;   // m
	float yaw; // degree(rad)
} Position;  // unit: m, rad

/**
 * @brief 速度信息
 * @param Vx x方向速度,m/s
 * @param Vy y方向速度,m/s
 * @param Vw 航向角速度,rad/s
 * @note Robot.Robot_in_self.velocity_exp. Robot为Robot_INFO变量
 */
typedef struct
{
	float Vx;
	float Vy;
	float Vw;
} Velocity; // unit: m/s, rad/s

/**
 * @brief 加速度信息
 * @param Ax x方向加速度,m/s^2
 * @param Ay y方向加速度,m/s^2
 * @param Aw 航向角加速度,rad/s^2
 */
typedef struct
{
	float Ax;
	float Ay;
	float Aw;
} Acceleration; // unit: m/s^2, rad/s^2

/**
 * @brief 机器人位置信息
 * @param position_now 当前位置信息
 * @param velocity_now 当前速度信息
 * @param position_exp 期望位置信息
 * @param velocity_exp 期望速度信息
 * @param position_last 上一次位置信息
 * @param velocity_last 上一次速度信息
 * @param acceleration_now 当前加速度信息
 *
 */
typedef struct
{
	Position position_now;
	Velocity velocity_now;
	Position position_exp;
	Velocity velocity_exp;
	Position position_last;
	Velocity velocity_last;
	Acceleration acceleration_now;
} Wheel_Pos;

/**
 * @brief 机器人信息
 * @param Robot_in_self 机器人在自身坐标系下的位置信息
 * @param Robot_in_world 机器人在世界坐标系下的位置信息
 * @param Robot_Contorl_Mode 机器人控制模式
 * @param Wheel_Speed 机器人轮子电机速度
 *
 */
typedef struct
{
	Wheel_Pos Robot_in_self;
	Wheel_Pos Robot_in_world;
	uint8_t Robot_Contorl_Mode;
	float Wheel_Speed[4];
	float Wheel_Angle[4];
	float Wheel_Speed_Abs[4];
	float Wheel_Direction[4];
	float Robot_Speed_max;
} Wheel_Data;

double calculate_angle(double vx, double vy);
void World_To_Self(Wheel_Data* robot);
void rubber_wheel_init(Wheel_Data* wheel,float max_speed);
void robot_velocity_to_rubber_position_to_wheel_speed(Wheel_Data* robot);
void Wheel_Data_To_Motor(Wheel_Data* robot,DJI_Motor_ControlTypeDef* rubber_motor, DJI_Motor_ControlTypeDef* wheel_motor);



#endif
